{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "%pylab inline\n",
    "from sympy import init_printing,Matrix,Symbol,symbols,pi\n",
    "init_printing(use_latex='mathjax') \n",
    "import sympy\n",
    "from sympyrobot import *\n",
    "import robot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 空间绕定轴转动讨论\n",
    "\n",
    "## 空间六维坐标\n",
    "\n",
    "$[px,py,pz,wx,wy,wz]^T$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "wx,wy,wz = sympy.symbols(\"wx wy wz\")\n",
    "px,py,pz = sympy.symbols(\"px,py,pz\")\n",
    "R = rpy2tr(wz,wy,wx)\n",
    "P = transl(px,py,pz)\n",
    "Tf = P*R\n",
    "Tf"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 绕定轴转动 Rodrigues 变换"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "theta,rx,ry,rz = symbols(\"theta r_x r_y r_z\")\n",
    "r = Matrix([rx,ry,rz])\n",
    "r # 转动轴"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "theta # 转动角度"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 反对称矩阵\n",
    "def ss(b):\n",
    "    return  Matrix([\n",
    "        [0, -b[2],  b[1]],\n",
    "        [b[2],  0,  -b[0]],\n",
    "        [-b[1], b[0],   0]])\n",
    "ss(r)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "R = cos(theta)*Matrix.eye(3)+(1-cos(theta))*r*r.transpose()+sin(theta)*ss(r)\n",
    "R"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "(R-R.transpose())/2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_Rodrigues():\n",
    "    theta,rx,ry,rz = sympy.symbols(\"theta r_x r_y r_z\")\n",
    "    r = Matrix([rx,ry,rz])\n",
    "    def ss(b):\n",
    "        return  Matrix([\n",
    "            [0, -b[2],  b[1]],\n",
    "            [b[2],  0,  -b[0]],\n",
    "            [-b[1], b[0],   0]])\n",
    "    R = cos(theta)*Matrix.eye(3)+(1-cos(theta))*r*r.transpose()+sin(theta)*ss(r)\n",
    "    return R\n",
    "    \n",
    "def my_Rodrigues(w):\n",
    "    real_theta = np.linalg.norm(w)\n",
    "    w = w/real_theta\n",
    "    R = get_Rodrigues()\n",
    "    return R.subs([(theta,real_theta),(rx,w[0]),(ry,w[1]),(rz,w[2])]).evalf()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "import cv2\n",
    "w = array([0.2,0.3,0.5])\n",
    "cv2.Rodrigues(w)[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "my_Rodrigues(w)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "w = w/np.linalg.norm(w) # 标准化"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 从给定的转动矩阵中得到转动轴和角"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "R = get_Rodrigues()\n",
    "R"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "[R[2,1]-R[1,2],\n",
    "R[0,2]-R[2,0],\n",
    "R[1,0]-R[0,1]]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "R[0,0]+R[1,1]+R[2,2]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "因为 $r_x^2+r_y^2+r_z^2=1$, 所以上式等于:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "2*cos(theta)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "综上可得,按照角轴表示与旋转坐标的转换公式:\n",
    " \n",
    "$$\n",
    "theta = arccos(\\frac{r_{11}+r_{22}+r_{33}-1}{2})\n",
    "$$\n",
    " \n",
    "$$\n",
    "\\hat{w}= \\frac{1}{2sin\\theta}\\begin{bmatrix}\n",
    "r_{32}-r_{23}\\\\ \n",
    "r_{13}-r_{31}\\\\ \n",
    "r_{21}-r_{12}\n",
    "\\end{bmatrix}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def uint(w):\n",
    "    return w/np.linalg.norm(w)\n",
    "\n",
    "def get_uint_w(R):\n",
    "    wx = R[2,1]-R[1,2]\n",
    "    wy = R[0,2]-R[2,0]\n",
    "    wz = R[1,0]-R[0,1]\n",
    "    w = array([wx,wy,wz])\n",
    "    w = uint(w)\n",
    "    return w"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "import cv2\n",
    "w = array([0.2,0.3,0.5])\n",
    "cv2.Rodrigues(w)[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "get_uint_w(cv2.Rodrigues(w)[0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "uint(w)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 绕定轴转动的矩阵特点:\n",
    "\n",
    "一个旋转矩阵可以看成绕一个单位向量 $k$ 转动 $\\theta$ 角,这种分解是唯一的,记为 $R(k,\\theta)$, 则有:\n",
    "\n",
    "1. $R$ 为单位正交矩阵: $R^T=R, |R| = 1$ \n",
    "2. $$\n",
    "R = U \\begin{bmatrix}\n",
    " 1 &  0 & 0 \\\\ \n",
    " 0 &  e^{-j\\theta} & 0 \\\\ \n",
    " 0 &  0 &  e^{j\\theta}\n",
    "\\end{bmatrix} U^{-1}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "w = array([0.2,0.3,0.5])\n",
    "w = unit(w)\n",
    "w"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "R = cv2.Rodrigues(w*1.2)[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "np.linalg.eig(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "e**(1.2j)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "e**(-1.2j)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# LArm正运动学\n",
    "\n",
    "讨论末端位姿为绕定轴转动的解"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def RotXYZMatrix(xAngle,yAngle,zAngle):\n",
    "    Rx = rotx(xAngle)\n",
    "    Ry = roty(yAngle)\n",
    "    Rz = rotz(zAngle)\n",
    "    R = Rx*Ry*Rz\n",
    "    return r2t(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def DH(a, alpha, d, theta):\n",
    "    from sympy import cos,sin\n",
    "    Taf = [[cos(theta),            -sin(theta),            0,              a],\n",
    "    [sin(theta)*cos(alpha), cos(theta)*cos(alpha),  -sin(alpha),    -sin(alpha)*d],\n",
    "    [sin(theta)*sin(alpha), cos(theta)*sin(alpha),  cos(alpha),     cos(alpha)*d],\n",
    "    [0,                     0,                      0,              1]]\n",
    "    return Matrix(Taf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "class Get_symbols(object):\n",
    "    def __init__(self):\n",
    "        symbols_dict={}\n",
    "        symbols_dict['theta'] = sympy.Symbol(\"theta_%d\")\n",
    "        thetas = [(sympy.Symbol(\"theta_%d\" % n), n)  for n in range(1,6)]\n",
    "        for theta, n in thetas:\n",
    "            symbols_dict[\"theta%d\" % n] = theta\n",
    "        params = [\"ShoulderOffsetY\",\n",
    "                  \"ShoulderOffsetZ\",\n",
    "                  \"ElbowOffsetY\",\n",
    "                  \"UpperArmLength\",\n",
    "                  \"LowerArmLength\",\n",
    "                  \"HandOffsetX\",\n",
    "                  \"HandOffsetZ\"\n",
    "                 ]\n",
    "        for p in params:\n",
    "            symbols_dict[p] = sympy.Symbol(p)\n",
    "        self.__dict__.update(symbols_dict)\n",
    "sb = Get_symbols()    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "class Left:\n",
    "    def __init__(self):\n",
    "        self.base = Matrix.eye(4)\n",
    "        self.base[1,3] = sb.ShoulderOffsetY\n",
    "        self.base[2,3] = sb.ShoulderOffsetZ\n",
    "        T1 = DH(0,-pi/2,0,sb.theta1)\n",
    "        T2 = DH(0,pi/2,0,sb.theta2+pi/2)\n",
    "        T3 = DH(sb.ElbowOffsetY,pi/2,sb.UpperArmLength,sb.theta3)\n",
    "        T4 = DH(0,-pi/2,0,sb.theta4)\n",
    "        T5 = DH(0,pi/2,0,sb.theta5)\n",
    "        self.T15 = T1*T2*T3*T4*T5\n",
    "        Tend1 = Matrix.eye(4)\n",
    "        Tend1[0,3] = sb.LowerArmLength + sb.HandOffsetX\n",
    "        Tend1[2,3] = -sb.HandOffsetZ\n",
    "        R = RotXYZMatrix(-pi/2,0,-pi/2)\n",
    "        self.tool = R*Tend1\n",
    "left=Left()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "class Right:\n",
    "    def __init__(self):\n",
    "        self.base = Matrix.eye(4)\n",
    "        self.base[1,3] = -sb.ShoulderOffsetY\n",
    "        self.base[2,3] = sb.ShoulderOffsetZ\n",
    "        T1 = DH(0,-pi/2,0,sb.theta1)\n",
    "        T2 = DH(0,pi/2,0,sb.theta2+pi/2)\n",
    "        T3 = DH(-sb.ElbowOffsetY,pi/2,sb.UpperArmLength,sb.theta3)\n",
    "        T4 = DH(0,-pi/2,0,sb.theta4)\n",
    "        T5 = DH(0,pi/2,0,sb.theta5)\n",
    "        self.T15 = T1*T2*T3*T4*T5\n",
    "        Tend1 = Matrix.eye(4)\n",
    "        Tend1[0,3] = sb.LowerArmLength + sb.HandOffsetX\n",
    "        Tend1[2,3] = -sb.HandOffsetZ\n",
    "        R = RotXYZMatrix(-pi/2,0,-pi/2)\n",
    "        self.tool = R*Tend1\n",
    "right=Right()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 绕定轴转动\n",
    "\n",
    "讨论, 假定已知末端矩阵,将其绕指定轴转动,设为绕 z 轴转动"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_Homg():\n",
    "    T = []\n",
    "    for i in range(3):\n",
    "        T.append([])\n",
    "        for j in range(4):\n",
    "            T[i].append(Symbol(\"r_{%d,%d}\" % (i+1,j+1)))\n",
    "    T.append([0,0,0,1])\n",
    "    return Matrix(T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 获取绕指定轴转动的后末端位姿符号矩阵\n",
    "def get_Tend():\n",
    "    T = get_Homg()\n",
    "    Trodrigues = Matrix.eye(4)\n",
    "    R = get_Rodrigues()\n",
    "    Trodrigues[:3,:3] = R\n",
    "    T = T*Trodrigues\n",
    "    return T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def tihuan(T):\n",
    "    # 为了简化表达,进行以下替换\n",
    "    sb = Get_symbols()\n",
    "    UpperArmLength = Symbol(\"l_1\")\n",
    "    ElbowOffsetY = Symbol(\"l_2\")\n",
    "    ShoulderOffsetY = Symbol(\"l_3\")\n",
    "    ShoulderOffsetZ = Symbol(\"l_4\")\n",
    "    HandOffsetX = Symbol(\"l_5\")\n",
    "    HandOffsetZ = Symbol(\"l_6\")\n",
    "    LowerArmLength = Symbol(\"l_7\")\n",
    "    return T.subs([(sb.ShoulderOffsetY,ShoulderOffsetY),\n",
    "                     (sb.ShoulderOffsetZ,ShoulderOffsetZ),\n",
    "                     (sb.ElbowOffsetY,ElbowOffsetY),\n",
    "                     (sb.UpperArmLength,UpperArmLength),\n",
    "                     (sb.LowerArmLength,LowerArmLength),\n",
    "                     (sb.HandOffsetX,HandOffsetX),\n",
    "                     (sb.HandOffsetZ,HandOffsetZ),\n",
    "                    ])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Tend = get_Tend()\n",
    "Base = left.base\n",
    "Tool = left.tool\n",
    "realT15 = Base.inv()*Tend*Tool.inv()\n",
    "realT15 = tihuan(realT15)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 求 $\\theta_1$\n",
    "\n",
    "参考标准的逆运动学求解有:\n",
    "\n",
    "``` python\n",
    "theta1 = atan(-T15[2,3]/T15[0,3])\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "世界上是得到了一个 $\\theta$ 和 $\\theta_1$ 的对应关系"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# tan(theta1)\n",
    "Eq(tan(sb.theta1),sympy.trigsimp(-realT15[2,3]/realT15[0,3]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "# RArm\n",
    "\n",
    "讨论末端位姿为绕定轴转动的解, 参考 LArm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Tend = get_Tend()\n",
    "Base = right.base\n",
    "Tool = right.tool\n",
    "realT15 = Base.inv()*Tend*Tool.inv()\n",
    "realT15 = tihuan(realT15)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Eq(tan(sb.theta1),sympy.trigsimp(-realT15[2,3]/realT15[0,3]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
